#include "platform.h"

#include "GenPID.h"



genpid_t pid ; 
int cnt = 300 ;
int32_t speed = 0 ;

#define N 8
genpid_t pidPool[N]; 
int32_t  speedPool[N] ;
int main() {
    //Init PID 
    for(uint8_t i=0;i<N;++i) {
        speedPool[i] = i ;
        GenPID_Init(&pidPool[i]  , 400 , 1000 ,0, 100+i) ;
        GenPID_SetOutMaxMin(&pidPool[i] ,-65536 , 65536) ;        
    }

    while (cnt--)
    {
        for(uint8_t i=0;i<N;++i) {
            GenPID_Update(&pidPool[i]  ,speedPool[i], 1) ;
            speedPool[i] +=  GenPID_GetOutput(&pidPool[i] ); 
            DEBUG_OUT = i ; 
            DEBUG_OUT = speedPool[i] ;     
        }
    }    
    return 0 ;
}

// int main() {
    
//     GenPID_Init(&pid  , 200 , 400 ,0, 666) ;
//     GenPID_SetOutMaxMin(&pid  ,-65536 , 65536) ;

//     while (cnt--)
//     {
//         GenPID_Update(&pid  ,speed, 1) ;
//         speed +=  GenPID_GetOutput(&pid);
//         DEBUG_OUT = speed ;
//     }
    

//     return 0 ;
// }





